﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using SceneLib;
using System.IO;
using System.Globalization;

namespace SplineLib
{
    public class SplineInfo
    {
        private CultureInfo cultureInfo = new CultureInfo("en-US");

        private string fileName;
        private List<Vector> points;

        public List<Vector> Points { get { return points; } }
        public float XMin { get; set; }
        public float YMin { get; set; }
        public float ZMin { get; set; }
        public float XMax { get; set; }
        public float YMax { get; set; }
        public float ZMax { get; set; }

        private float tension;
        private Matrix matrix;

        public SplineInfo(string fileName, float tensionFactor)
        {
            this.fileName = fileName;
            this.points = new List<Vector>();
            tension = tensionFactor;
            matrix = new Matrix();
            
            //TODO matriz de carga
            matrix.M[0, 0] = 0.0f;
            matrix.M[0, 1] = 1.0f;
            matrix.M[0, 2] = 0.0f;
            matrix.M[0, 3] = 0.0f;

            matrix.M[1, 0] = -tension;
            matrix.M[1, 1] = 0.0f;
            matrix.M[1, 2] = tension;
            matrix.M[1, 3] = 0.0f;

            matrix.M[2, 0] = 2.0f * tension;
            matrix.M[2, 1] = tension - 3.0f;
            matrix.M[2, 2] = 3.0f - 2.0f * tension;
            matrix.M[2, 3] = -tension;

            matrix.M[3, 0] = -tension;
            matrix.M[3, 1] = 2.0f - tension;
            matrix.M[3, 2] = tension - 2.0f;
            matrix.M[3, 3] = tension;
            XMin = YMin = ZMin = Single.PositiveInfinity;
            XMax = YMax = ZMax = Single.NegativeInfinity;            
        }

        public void Load()
        {
            using (StreamReader sr = new StreamReader(fileName))
            {
                string line = sr.ReadLine();
                while (line != null)
                {
                    string[] stringValues = line.Split(' ');
                    if (stringValues.Length == 3)
                    {
                        float x = float.Parse(stringValues[0], NumberStyles.Any, cultureInfo);
                        float y = float.Parse(stringValues[1], NumberStyles.Any, cultureInfo);
                        float z = float.Parse(stringValues[2], NumberStyles.Any, cultureInfo);
                        Vector point = new Vector(x, y, z);

                        if (x <= XMin)
                            XMin = x;
                        if (y <= YMin)
                            YMin = y;
                        if (z <= ZMin)
                            ZMin = z;

                        if (x >= XMax)
                            XMax = x;
                        if (y >= YMax)
                            YMax = y;
                        if (z >= ZMax)
                            ZMax = z;

                        points.Add(point * 10);

                    }
                    line = sr.ReadLine();
                }
            }
        }

        public static Vector Circle(float t, float radius, float h)
        {
            float x = radius * Convert.ToSingle(Math.Sin(t));
            float z = radius * Convert.ToSingle(Math.Cos(t));
            return new Vector(x, h, z);
        }

        public Vector SplineFunction(float t)
        {
            t += 1;
            if (Points.Count >= 4)
            {
                int ti = Convert.ToInt32(Math.Floor(t));
                int size = Points.Count - 1;

                Vector p1 = Points[(size + ti - 1) % size];
                Vector p2 = Points[ti % size];
                Vector p3 = Points[(ti + 1)%size];
                Vector p4 = Points[(ti + 2) % size];

                Vector xVec = new Vector(p1.x, p2.x, p3.x, p4.x);
                Vector yVec = new Vector(p1.y, p2.y, p3.y, p4.y);
                Vector zVec = new Vector(p1.z, p2.z, p3.z, p4.z);

                xVec = matrix * xVec;
                yVec = matrix * yVec;
                zVec = matrix * zVec;

                t -= ti;

                return new Vector(xVec.w * t * t * t + xVec.z * t * t + xVec.y * t + xVec.x,
                                    yVec.w * t * t * t + yVec.z * t * t + yVec.y * t + yVec.x,
                                    zVec.w * t * t * t + zVec.z * t * t + zVec.y * t + zVec.x);
            }
            else
                return new Vector();
        }

        public Vector SplineFunctionPrime(float t)
        {
            t += 1;
            if (Points.Count >= 4)
            {
                int ti = Convert.ToInt32(Math.Floor(t));
                int size = Points.Count - 1;

                Vector p1 = Points[(size + ti - 1) % size];
                Vector p2 = Points[ti % size];
                Vector p3 = Points[(ti + 1) % size];
                Vector p4 = Points[(ti + 2) % size];

                Vector xVec = new Vector(p1.x, p2.x, p3.x, p4.x);
                Vector yVec = new Vector(p1.y, p2.y, p3.y, p4.y);
                Vector zVec = new Vector(p1.z, p2.z, p3.z, p4.z);

                xVec = matrix * xVec;
                yVec = matrix * yVec;
                zVec = matrix * zVec;

                t -= ti;

                return new Vector(3 * xVec.w * t * t + 2 * xVec.z * t + xVec.y,
                                   3 * yVec.w * t * t + 2 * yVec.z * t + yVec.y,
                                    3 * zVec.w * t * t + 2 * zVec.z * t + zVec.y);
            }
            else
                return new Vector();
        }

        public Vector SplineFunctionDoublePrime(float t)
        {
            t += 1;
            if (Points.Count >= 4)
            {
                int ti = Convert.ToInt32(Math.Floor(t));
                int size = Points.Count - 1;

                Vector p1 = Points[(size + ti - 1) % size];
                Vector p2 = Points[ti % size];
                Vector p3 = Points[(ti + 1) % size];
                Vector p4 = Points[(ti + 2) % size];

                Vector xVec = new Vector(p1.x, p2.x, p3.x, p4.x);
                Vector yVec = new Vector(p1.y, p2.y, p3.y, p4.y);
                Vector zVec = new Vector(p1.z, p2.z, p3.z, p4.z);

                xVec = matrix * xVec;
                yVec = matrix * yVec;
                zVec = matrix * zVec;

                t -= ti;

                return new Vector(6 * xVec.w * t + 2 * xVec.z,
                                   6 * yVec.w * t + 2 * yVec.z,
                                    6 * zVec.w * t + 2 * zVec.z);
            }
            else
                return new Vector();
        }
    }
}
